/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_velocity.h"
#include "foc_pid.h"
#include "foc_model.h"
#include "foc_para.h"
#include "foc_filter.h"
#include "foc_util.h"

#include <string.h>

FOC_PID_S PidVel;
int16_t MotorCurVel;
int16_t MotorTargetVel = -50;
int16_t VelFilter = 0;
uint16_t VelCallPeriod = 0;
KF1DimObj Kfobj;

int32_t computeVelocity(uint16_t curencoder)
{
    static uint16_t cnt = 0;
    static uint8_t computeLevel = 0;//0~4级，采样速度逐渐加快
    static uint16_t lastencoder = 0;
    static int16_t realvel = 0;
    uint16_t targetcnt = 0;
    int16_t veltemp = 0;
    int8_t dir = 0;
    /*
    switch(computeLevel) {//根据速度自动调整采样间隔，提高准确度
        case 0:
            targetcnt = 15;//每16ms计算一次，最小转速63/s
            break;
        case 1:
            targetcnt = 7;//每8ms计算一次，最小转速125/s
            break;
        case 2:
            targetcnt = 3;//每4ms计算一次，最小转速250/s
            break;
        case 3:
            targetcnt = 1;//每2ms计算一次，最小转速500/s
            break;
        case 4:
            targetcnt = 0;//每1ms计算一次，最小转速1000/s
            break;
        default:
            break;
    }
    cnt++;
    if(cnt > targetcnt) {
        veltemp = (curencoder - lastencoder) / cnt;
        if(veltemp == 0) {
            if(computeLevel > 0) {
                computeLevel--;
            } else {
                realvel = 0;
            }
        } else {
            realvel = veltemp;
            if(veltemp > 1000)
                computeLevel = 0;
            else if(veltemp > 500)
                computeLevel = 1;
            else if(veltemp > 250)
                computeLevel = 2;
            else if(veltemp > 125)
                computeLevel = 3;
            else
                computeLevel = 4;
        }
        cnt = 0;
        lastencoder = curencoder;
    }
    */
    cnt++;
    
    if(cnt > 0) {//1ms计算一次速度
        realvel = FOC_ABS(curencoder, lastencoder);
        if((curencoder > lastencoder &&  realvel < ANGLE2PAHSE(180)) || \
            (curencoder < lastencoder &&  realvel > ANGLE2PAHSE(180)))
            dir = 1;
        else 
            dir = -1;
        if(realvel > ANGLE2PAHSE(180))
            realvel = ANGLE2PAHSE(360) - realvel;
        realvel = dir * realvel;
        MotorWorkPara.AccumulatePhase += realvel;
        realvel = realvel / cnt / VelCallPeriod;
        //realvel = filterKalman1DimCalc(&Kfobj, realvel);
        lastencoder = curencoder;
        cnt = 0;
    }
    //printf("%d\r\n", MotorWorkPara.AccumulatePhase);
    return realvel;
}

void focVelocityLoop(uint16_t period)
{
    int32_t targetCurr = 0;
    int16_t convertPhase = focConvertPhase(getCurPahse());    
    VelCallPeriod = period;
    MotorCurVel = computeVelocity(convertPhase);
    if(MotorCurVel < 0 && MotorCurVel > 0) {
        PidVel.Kd = 50;
        targetCurr = pidCtrlBlock(MotorTargetVel, MotorCurVel, &PidVel);
    } else {
        PidVel.Kd = 0;
        VelFilter = lowPassFilter1Order(800, MotorCurVel, VelFilter);
        targetCurr = pidCtrlBlock(MotorTargetVel, VelFilter, &PidVel);
    }
    //printf("%d,%d,%d\r\n", curtargetvel, VelFilter, targetCurr);
    focUpdateCurrQ(targetCurr);
}

void focVelocityInit(void)
{
    memset(&PidVel, 0, sizeof(PidVel));
    PidVel.Kd = 100;
    PidVel.Ki = 200;
    PidVel.Kd = 0;
    PidVel.IntegralMax = 300;
    PidVel.IntegralMin = -300;
    PidVel.OutputMax = 300;
    PidVel.OutputMin = -300;
    PidVel.LastInput = 0;
    PidVel.Divide = 1024;
    VelFilter = 0;

    Kfobj.LastP = 20;
    Kfobj.Now_P = 0;
    Kfobj.out = 0;
    Kfobj.Kg = 0;
    Kfobj.Q = 1;
    Kfobj.R = 543;
}

void focUpdateVel(int32_t targetvel)
{
    MotorTargetVel = targetvel;
}